#!/usr/bin/env python
"""
Basic layout of robot:
		__W2__
     /      \
    /        \
    \        /
	W1--------W3

This robot is an omni-directional robot, built around a hexagonal shape
Each of the wheels are 135 degrees apart from one another.
Rotation in the clockwise direction is viewed as forward motion.
With rotation in the counter-clockwise direction is viewed as reverse motion

"""
import serial
import socket
import sys
import array
import struct
import time

ser = serial.Serial()
ser.port = "/dev/ttyACM0"
ser.baudrate = 9600
ser.timeout = 0
ser.open()

##Global variables
byte0 = int(0xFE)
byte1 = int(0xFF)
bytef = int(0xEA)

m1 = int(0)
m2 = int(0)
m3 = int(0)

## IDs for the motor drivers, these should not change
id2 = int(0x0D)
id3 = int(0x1E)
id1 = int(0x19)

## hex values for forward and reverse
fwd = int(6)
rev = int(5)

## variables for motor directions
dir1 = fwd
dir2 = fwd
dir3 = fwd

## Socket Object in python
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

def hundred():
	global m1, m2, m3, dir1, dir2, dir3
	m1 = int(90)
	m2 = int(0)
	m3 = int(90)
	dir1 = fwd
	dir3 = rev

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)


def motor1():
	global m1, m2, m3, dir1, dir2, dir3
	if (m1 <= 0):
		m1 = int(15)
	elif (m1 <= int(100)):
		m1 += int(5)
	m2 = int(0)
	m3 = int(0)
	dir1 = fwd

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)

def motor2():
	global m1, m2, m3, dir1, dir2, dir3
	if (m2 <= 0):
		m2 = int(15)
	elif (m2 <= int(100)):
		m2 += int(5)
	m1 = int(0)
	m3 = int(0)
	dir2 = fwd

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)

def motor3():
	global m1, m2, m3, dir1, dir2, dir3
	if (m3 <= 0):
		m3 = int(15)
	elif (m3 <= int(100)):
		m3 += int(5)
	m1 = int(0)
	m2 = int(0)
	dir3 = fwd

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)

def irStuff():
	irByte0 = int(0xaa)
	irByte1 = int(0x00)
	irByte2 = int(0xff)
	irByte3 = int(0x00)

	packet_int = struct.pack('BBBBBBBBBBBB', irByte0, irByte1, irByte2, irByte3, irByte3, irByte3, irByte3, irByte3, irByte3, irByte3, irByte3)
	s.send(packet_int)

## Connects to the server
def connectSvr():
	host = '192.168.1.2'
	port = 50000
	size = 12
	
	try:
		s.connect((host,port))
		print '\nConnecting to Server...\n'
	except Exception, e:
		alert('Something is wrong with %s:%d. Exception is type %s' %(host, port, e))
	print 'Connection Established!!!\n'
## End of Connect Svr

# prints and identifies exception type for connection issues
def alert(msg):
	print >>sys.stderr, msg
	sys.exit(1)

## this is for angle 0
def wButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is going in any other direction than w
	if ((dir1 != fwd) or (dir3 != rev) or (int(m1) != int(m3))):
		m1 = int(10)
		m2 = int(0)
		m3 = int(10)
		dir1 = fwd
		dir2 = fwd
		dir3 = rev
	else: ## This is the default case for going forward
		if (int(m1) <= int(90)):
			m1 += int(5)
		if (int(m3) <= int(90)):
			m3 += int(5)
		dir1 = fwd
		dir2 = fwd
		dir3 = rev
		m2 = int(0)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)

## this is for angle 180
def sButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is going in any other diretion than reverse
	if ((dir1 != rev) or (dir3 != fwd) or (int(m1) != int(m3))):
		m1 = int(10)
		m2 = int(0)
		m3 = int(10)
		dir1 = rev
		dir2 = fwd
		dir3 = fwd
	
	else: ## This is the default case for going forward
		if (int(m1) <= int(90)):
			m1 += int(5)
		if (int(m3) <= int(90)):
			m3 += int(5)
		m2 = int(0)	
		dir1 = rev
		dir2 = fwd
		dir3 = fwd
		m2 = int(0)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)

## this is for angle 45
def eButton():
	global m1, m2, m3, dir1, dir2, dir3
	## if the robot is going any other direction than 45
	if ((dir2 != fwd) or (dir3 != rev) or (int(m2) != int(m3))):
		m1 = int(0)
		m2 = int(10)
		m3 = int(10)
		dir1 = fwd
		dir2 = fwd
		dir3 = rev
	else: ## This is the default case for going forward
		if (m2 <= int(90)):
			m2 += int(5)
		if (m3 <= int(90)):
			m3 += int(5)		
		dir1 = fwd
		dir2 = fwd
		dir3 = rev
		m1 = int(0)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
## End of 45 degree


## This is for angle 90, this one is tricky
def dButton():
	global m1, m2, m3, dir1, dir2, dir3
	if((dir1 != rev) or (dir2 != fwd) or (dir3 != rev)):
		m1 = int(10)
		m2 = int(20)
		m3 = int(10)
		dir1 = rev
		dir2 = fwd
		dir3 = rev
	else:
		if((m1 == 0) or (m2 == 0) or (m3 == 0)):
			m1 = int(10)
			m2 = int(20)
			m3 = int(10)
		elif(int(m2) <= int(90)):
			m2 += int(10)
			m1 += int(5)
			m3 += int(5)
		dir1 = rev
		dir2 = fwd
		dir3 = rev

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
## End of angle 90

## This is for movement for angle -90
def aButton():
	global m1, m2, m3, dir1, dir2, dir3

	if((dir1 != fwd) or (dir2 != rev) or (dir3 != fwd)):
		m1 = int(10)
		m2 = int(20)
		m3 = int(10)
		dir1 = fwd
		dir2 = rev
		dir3 = fwd
	else:
		if((m1 == 0) or (m2 == 0) or (m3 == 0)):
			m1 = int(5)
			m2 = int(10)
			m3 = int(5)
		elif(int(m2) <= int(90)):
			m2 += int(10)
			m1 += int(5)
			m3 += int(5)
		dir1 = fwd
		dir2 = rev
		dir3 = fwd

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
## End of angle -90

## This is for counte-clockwise rotation
def ccwButton():
	global m1, m2, m3
	m1 = m2 = m3 = int(25)
	dir1 = dir2 = dir3 = fwd
	
	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)

## This is for clockwise rotation
def cwButton():
	global m1, m2, m3
	m1 = m2 = m3 = int(25)
	dir1 = dir2 = dir3 = rev	

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)


## This is for angle 135
def xButton():
	global m1, m2, m3, dir1, dir2, dir3

	if ((dir1 != rev) or (dir2 != fwd) or (int(m1) != int(m2))):
		m1 = int(10)
		m2 = int(10)
		m3 = int(0)
		dir1 = rev
		dir2 = fwd
	else: ## This is the default case for going forward
		if (m1 <= int(90)):
			m1 += int(5)
		if (m2 <= int(90)):
			m2 += int(5)		
		dir1 = rev
		dir2 = fwd
		dir3 = fwd
		m3 = int(0)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
## End of angle 135

## This is for angle -135
def zButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going any direction but -135

	if ((dir2 != rev) or (dir3 != fwd) or (int(m2) != int(m3))):
		m2 = int(10)
		m3 = int(10)
		m1 = int(0)
		dir2 = rev
		dir3 = fwd
	else: ## This is the default case for going forward
		if (m2 <= int(90)):
			m2 += int(5)
		if (m3 <= int(90)):
			m3 += int(5)		
		dir2 = rev
		dir3 = fwd
		dir1 = fwd
		m1 = int(0)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
## End of angle -135

## Movement for angle -45
def qButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the -135 degree direction

	if ((dir1 != fwd) or (dir2 != rev) or (int(m1) != int(m2))):
		m1 = int(10)
		m2 = int(10)
		m3 = int(0)
		dir1 = fwd
		dir2 = rev
	else: ## This is the default case for going forward
		if (m1 <= int(90)):
			m1 += int(5)
		if (m2 <= int(90)):
			m2 += int(5)		
		dir1 = fwd
		dir2 = rev
		dir3 = fwd
		m3 = int(0)
	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
## end of angle -45

def stp():
	global m1, m2, m3
	m1 = int(0)
	m2 = int(0)
	m3 = int(0)
	dir1 = fwd
	dir2 = fwd
	dir3 = fwd
	
	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3, bytef)
	s.send(packet_int)
def main():
	running = 1
	while running:
		uInput = raw_input('Junk and stuff\n')
		## angle 0
		if uInput == 'w':
			wButton()
		## angle 180
		elif uInput == 's':
			sButton()
		## Counter-Clockwise Rotation
		elif uInput == '.':
			ccwButton()
		## Clockwise rotation
		elif uInput == ',':
			cwButton()
		## angle 90
		elif uInput == 'd':
			dButton()
		## angle 45
		elif uInput == 'e':
			eButton()
		## angle 135
		elif uInput == 'x':
			xButton()
		## angle -135
		elif uInput == 'z':
			zButton()
		## angle -90
		elif uInput == 'a':
			aButton()
		## Angle -45
		elif uInput == 'q':
			qButton()
		elif uInput == 'ir':
			irStuff()
		elif uInput == 'm1':
			motor1()
		elif uInput == 'm2':
			motor2()
		elif uInput == 'm3':
			motor3()
		elif uInput == 'f':
			hundred()	
		## Stop motors
		elif uInput == 'st':
			stp()
		else:
			s.close()
			exit()
connectSvr()
main()

